

#include "TimeFunction.h"
#include <assert.h>
#include <iostream>
#include <cmath>
#include "MyTime.h"
#include "util.h"

using namespace std;

extern MyTime myTime;

//int TimeFunction::id = 0;


TimeFunction::TimeFunction()
{
  id = -1;
  nblocks = 0;

  return;
}



TimeFunction::~TimeFunction()
{
  return;
}



void TimeFunction::addTimeBlock(std::vector<double> & ps_)
{
    assert(ps_.size() >= 10);

    t0.push_back(ps_[0]);
    tf.push_back(ps_[1]);

    int i, size = ps_.size()-2;
    std::vector<double>  doublelist(size);

    for(int i=0; i<size; ++i)
        doublelist[i] = ps_[i+2];

    ps.push_back(doublelist);

    nblocks = ps.size();

    return;
}




void TimeFunction::update()
{
  double t = myTime.cur, tmt0;
  std::vector<double>  p;

  factor = 0.;
  for(int i=0; i<nblocks; ++i)
  {
      if( (t < t0[i]-tol)  || (t > tf[i]-tol) )
        continue;

      p = ps[i];

      tmt0 = t-t0[i];

      factor += ( p[0] + p[1]*tmt0 + p[2]*sin(p[3]*tmt0+p[4]) + p[5]*cos(p[6]*tmt0+p[7]) );
  }

  return;
}



void TimeFunction::printSelf()
{
  if (nblocks == 0) return;

  std::vector<double>  p;

  for(int i=0; i<nblocks; ++i)
  {
    p = ps[i];

    std::cout << " id = " << id <<
    ": " << t0[i] << " <= t < " << tf[i] <<
    " :  lam[" << i+1 << "](t) = " << p[0] << " + " << p[1] << " * t + " 
             << p[2] << " * sin(" << p[3] << "*t+" << p[4] << ") + "
             << p[5] << " * cos(" << p[6] << "*t+" << p[7] << ")\n";
  }

  return ;
}




double TimeFunction::eval(int i, double tcur)
{
    return 0.0;
}




double TimeFunction::evalFirstDerivative(int i, double tcur)
{
    return 0.0;
}




double TimeFunction::evalSecondDerivative(int i, double tcur)
{
    return 0.0;
}







